#include <MovingParticle.h>
#include <vector>
#include <set>
#include <limits>
using namespace std;
#include <mex.h> //need for premature exit when something goes wrong.

#include <szmexutilitytemplate.h>
#include <szMiscOperations.h>
#include <IntersectionConvexPolygons.h>

void 
MovingParticle::print(char* tab) const
{
	printf("%s%d %3.3f %3.3f %3.3f %3.3f %3.3f %3.3f\n", tab, id, p0.m_X, p0.m_Y, p.m_X, p.m_Y, v[0], v[1]);
}

vector<MovingParticle*>
MovingParticle::vectorize(MovingParticle* start)
{
	vector<MovingParticle*> tr;
	MovingParticle* p = start;
	set<MovingParticle*> pset;
	bool success = true;
	do
	{
		tr.push_back(p);
		if (pset.find(p) != pset.end()) //premature loop is found
		{
			success = false;
			break;
		}
		if (p->next == NULL)
		{
			success = false;
			break;
		}
		pset.insert(p);
		p = p->next;
	} while (p != start);
	if (success == false)
	{
		printf("failed vectorization data.\n");
		for (int i = 0; i<tr.size(); ++i)
		{
			printf("%d %f %f %d\n", i + 1, tr[i]->p.m_X, tr[i]->p.m_Y, tr[i]->id);
		}
		mexErrMsgTxt("vectorize: failed to vectorize a shape.");
		//tr.clear();
	}
	return tr;
}

MovingParticle*
MovingParticle::getNextEvent()
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	MovingParticle* p = NULL;
	float time = std::numeric_limits<float>::infinity();
	for (set<MovingParticle*>::iterator it = factory->activeSet.begin(); it != factory->activeSet.end(); it++)
	{
		if ((*it)->event.t < time)
		{
			p = *it;
			time = p->event.t;
		}
	}
	return p;
}

bool
calculateBisectorVelocity(CParticleF a, CParticleF o, CParticleF b, float& vx, float& vy)
{
	if (o == a) //leaf node
	{
		float ang = GetVisualDirection(o.m_X, o.m_Y, b.m_X, b.m_Y) + PI/2.0;
		a.m_X = o.m_X + cos(ang);
		a.m_Y = o.m_Y + sin(ang);
	}
	else if (o == b) //leaf node
	{
		float ang = GetVisualDirection(o.m_X, o.m_Y, a.m_X, a.m_Y) - PI / 2.0;
		b.m_X = o.m_X + cos(ang);
		b.m_Y = o.m_Y + sin(ang);
	}
	CParticleF bs = bisector(o, a, b);
	float ang = GetVisualAngle2(a.m_X, a.m_Y, b.m_X, b.m_Y, o.m_X, o.m_Y);
	float cs = cos((PI - Abs(ang)) / 2.0);
	bool bret = true;
	if (cs < 0.01)
	{
		cs = 0.01;
		bret = false;
	}
	float len = 1.0f / cs;
	bs.m_X *= len;
	bs.m_Y *= len;
	vx = bs.m_X;
	vy = bs.m_Y;
	return bret;
}

bool clockWise(vector<MovingParticle*>& particles)
{
	vector<CParticleF> pnts;
	for (int i = 0; i<particles.size(); ++i)
	{
		pnts.push_back(particles[i]->getP());
	}
	return ClockWise(pnts);
}


/*
Find the time to hit for the 1st particle (P) to a slanted plane formed with Q and R.
Returns Inf if no intersection within the bounded polygon.
*/
float
MovingParticle::intersectSideAndLine(const MovingParticle* p, const MovingParticle* q, const MovingParticle* r)
{
	CParticleF po = p->p;
	CParticleF qo = q->p;
	CParticleF ro = r->p;
	//perp to plane
	float eps = 1.0e-5;
	float t = std::numeric_limits<float>::infinity();
	{
		float d = 100.0f; //Max(Distance(po, qo), Distance(po, ro)); //find the bounding limit
		CParticleF poly[3];
		//make a 3D plane with a unit-slant
		poly[0] = qo; 
		poly[1] = ro;
		poly[2].m_X = qo.m_X + d * q->v[0];
		poly[2].m_Y = qo.m_Y + d * q->v[1];
		poly[2].m_Z = d;
		CParticleF z = perp2Plane(poly[0], poly[1], poly[2]);
		CParticleF u(p->v[0], p->v[1], 1.0f);
		float t0 = intersectPlaneAndLine(qo, z, po, u);
		if (t0 > 0)
		{
			//now check if the intersection is within the line segment
			pair<float, float> param = _IntersectConvexPolygon::intersect(p->p, p->move(t0), q->move(t0), r->move(t0));
			if (param.first > 0.9 && param.first < 1.1 && param.second > 0 && param.second <= 1.0f)
			{
				t = t0;
			}
		}
	}
	return t;
}

EventStruct
MovingParticle::findNextEdgeEvent() const
{
	CParticleF p2(p.m_X + v[0], p.m_Y + v[1]);
	CParticleF q0 = next->p;
	CParticleF q2(q0.m_X + next->v[0], q0.m_Y + next->v[1]);
	pair<float,float> param = _IntersectConvexPolygon::intersect(p, p2, q0, q2);
	EventStruct ev(std::numeric_limits<float>::infinity(), CollisionEvent, this, this->next);
	if (param.first > 0 && param.second > 0)
	{
		ev.t = time + param.first;
	}
	return ev;
}

EventStruct
MovingParticle::findNextSplitEvent() const
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	EventStruct ev(std::numeric_limits<float>::infinity(), SplitEvent, this);
	for (set<MovingParticle*>::iterator j = factory->activeSet.begin(); j != factory->activeSet.end(); ++j)
	{
		const MovingParticle* q = *j;
		const MovingParticle* r = q->next;
		if (this->id == 381 && (q->id == 403 || r->id == 403))
		{
			ev.t += 0;
		}
		if (this == q || this->prev == q) continue;

		//if (eta <= 0) continue;

		float t0 = intersectSideAndLine(this, q, r); // period - p->time);
		if (t0 >= 0)
		{
			float t = t0 + time;
			if (t < ev.t)
			{
				ev.t = t;
				ev.q = q;
				ev.r = r;
			}
		}
	}
	return ev;
}


bool
MovingParticle::updateEvent()
{
	//check if the event needs to be updated. If not, then simply return.
	if (event.type == CollisionEvent && next == event.q) return false;
	if (event.type == SplitEvent && event.q->bActive && event.q->next == event.r) return false;

	event = findNextEdgeEvent();
	EventStruct ev2 = findNextSplitEvent();
	bool bret = false;
	/*if (ev1.t < event.t)
	{
		event = ev1;
		bret = true;
	}*/
	if (ev2.t < event.t)
	{
		event = ev2;
		bret = true;
	}
	return bret;
}

bool 
MovingParticle::applyEvent()
{
	MovingParticle* p = this;
	MovingParticle* q = (MovingParticle*)event.q;
	MovingParticle* r = (MovingParticle*)event.r;
	ParticleFactory* factory = ParticleFactory::getInstance();
	MovingParticle* pnew[2] = { NULL, NULL };
	if (event.type == CollisionEvent)
	{
		pnew[0] = factory->makeParticle(p->p, Collide, event.t);
		setNeighbors(pnew[0], p->prev, p->next->next);
		factory->inactivate(p);
		factory->inactivate(q);
		//setRelation(this, pnew);
		//setRelation(this->next, pnew);
	}
	else if (event.type == SplitEvent)
	{
		pnew[0] = factory->makeParticle(this->p, Split, event.t);
		pnew[1] = factory->makeParticle(this->p, Split, event.t);
		setNeighbors(pnew[0], event.p->prev, r);
		setNeighbors(pnew[1], q, event.p->next);
		factory->inactivate(p);
		///TK - need to implement this.
		///setRelation(p, pnew[0]);
		///setRelation(p, pnew[1]);
	}
	for (int i = 0; i<2; ++i)
	{
		if (pnew[i] == NULL) continue;
		if (factory->activeSet.find(pnew[i]) == factory->activeSet.end()) continue; //must be a part of the previous trace. we are done.
		if (pnew[i]->bInitialized == true) continue; //must be a part of the previous trace. we are done.
		vector<MovingParticle*> vp = vectorize(pnew[i]);
		if (vp.empty()) return false;

		/*printf("Before removing unstables.\n");
		for (int k = 0; k < vp.size(); ++k)
		{
			vp[k]->print("\t");
		}*/
		vp = removeUnstable(vp);
		float vx, vy;
		for (int k = 0; k < vp.size(); ++k)
		{
			if (vp[k]->bInitialized == false)
			{
				//if (calculateBisectorVelocity(pnew[i]->prev->p, pnew[i]->p, pnew[i]->next->p, vx, vy) == false)
				if (calculateBisectorVelocity(vp[k]->prev->p, vp[k]->p, vp[k]->next->p, vx, vy) == false)
				{
					printf("applyEvent: failed to set velocity.\n");
					for (int k = 0; k < vp.size(); ++k)
					{
						vp[k]->print();
					}
					//mexErrMsgTxt("applyEvent: failed to set velocity.");
					return false;
				}
				else
				{
					vp[k]->setVelocity(vx, vy);
				}
			}
		}
	}
	return true;
}

vector<MovingParticle*> 
MovingParticle::removeUnstable(vector<MovingParticle*> particles)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	float v[2];
	while (particles.size() >= 3)
	{
		bool bChanged = false;
		for (int i = 0; i<particles.size(); ++i)
		{
			MovingParticle* p0 = particles[i];
			float vx, vy;
			if (calculateBisectorVelocity(p0->prev->p, p0->p, p0->next->p, vx, vy) == false)
			{
				factory->inactivate(p0);
				MovingParticle* prev = p0->prev;
				MovingParticle* next = p0->next;
				float dn = Distance(p0->p, next->p);
				float dp = Distance(p0->p, prev->p);
				if (dn <= dp)
				{
					MovingParticle* q = factory->makeParticle(next->p, Merge, p0->time); // p0->next->type, time);
					setNeighbors(q, prev, next->next);
					//setRelation(next, q);
					//q->bDirty = true;
					factory->inactivate(next);
					//next = q; //IMPORTANT ?
					particles = vectorize(q);
				}
				///TK! - Do we need both when dp==dn??  I am changing it to else-if since this is causing a problem.
				//if (dp <= dn) 
				else if (dp <= dn)
				{
					MovingParticle* p = factory->makeParticle(prev->p, Merge, p0->time); // p0->prev->type, time);
					setNeighbors(p, prev->prev, next);
					//setRelation(prev, p);
					//p->bDirty = true;
					factory->inactivate(prev);
					//p->setVelocity();
					//prev->bActive = false;
					particles = vectorize(p);
				}
				bChanged = true;

				break;
			}
		}
		if (bChanged == false) break;
	}

	if (particles.size() <= 3)
	{
		for (int i = 0; i < particles.size(); ++i)
		{
			factory->inactivate(particles[i]);
		}
		particles.clear();
	}
	else
	{
		for (int i = 0; i < particles.size(); ++i)
		{
			if (particles[i]->bInitialized == false)
			{
				float vx, vy;
				if (calculateBisectorVelocity(particles[i]->prev->p, particles[i]->p, particles[i]->next->p, vx, vy) == false)
				{
					printf("unstability resits in removeUnstable.\n");
					vector<MovingParticle*> vp = vectorize(particles[i]);
					for (int k = 0; k < vp.size(); ++k)
					{
						vp[k]->print();
					}
					mexErrMsgTxt("removeUnstable: failed to set velocity.");
				}
				else
				{
					particles[i]->setVelocity(vx, vy);
				}
			}
		}
	}
	return particles;
}

vector<vector<CParticleF>> 
MovingParticle::takeSnapshots()
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	set<MovingParticle*> pset;
	vector<vector<CParticleF>> snapshots;
	while (true)
	{
		bool bdone = true;
		for (set<MovingParticle*>::iterator it = factory->activeSet.begin(); it != factory->activeSet.end(); ++it)
		{
			MovingParticle* p = *it;
			if (pset.find(p) == pset.end())
			{
				vector<MovingParticle*> vp = vectorize(p);
				vector<CParticleF> shot;
				for (int i = 0; i < vp.size(); ++i)
				{
					pset.insert(vp[i]);
					shot.push_back(vp[i]->p);
				}
				bdone = false;
				snapshots.push_back(shot);
			}
		}
		if (bdone) break;
	}
	return snapshots;
}

//remove unstable particles and polygons with less than 4 vertices.
void 
MovingParticle::tidyUp()
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	set<MovingParticle*> pset;
	while (true)
	{
		bool bdone = true;
		for (set<MovingParticle*>::iterator it = factory->activeSet.begin(); it != factory->activeSet.end(); ++it)
		{
			MovingParticle* p = *it;
			if (pset.find(p) == pset.end())
			{
				vector<MovingParticle*> vp = vectorize(p);

				vp = removeUnstable(vp);
				for (int i = 0; i < vp.size(); ++i)
				{
					pset.insert(vp[i]);
				}
				if (vp.size() <= 3)
				{
					for (int i = 0; i < vp.size(); ++i)
					{
						factory->inactivate(vp[i]);
					}
				}
				bdone = false;
				break; //active set has changed. the interator needs to be initialized again.
			}
		}
		if (bdone) break;
	}
}

bool
MovingParticle::sanityCheck()
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	bool bdone = true;
	for (set<MovingParticle*>::iterator it = factory->activeSet.begin(); it != factory->activeSet.end(); ++it)
	{
		CParticleF p = (*it)->p;
		CParticleF p2 = (*it)->next->p;

		//NOTE: j starts j+2 to skip the adjacent side
		set<MovingParticle*>::iterator it2 = it;
		it2++;
		for (; it2 != factory->activeSet.end(); ++it2)
		{
			CParticleF q = (*it2)->p;
			CParticleF q2 = (*it2)->next->p;
			pair<float, float> param = _IntersectConvexPolygon::intersect(p, p2, q, q2);
			if (param.first>0 && param.first<0.9 && param.second>0 && param.second < 1.0)
			{
				printf("sanityCheck(): crossing at %d(%3.3f,%3.3f)-%d(%3.3f,%3.3f) and %d(%3.3f,%3.3f)-%d(%3.3f,%3.3f) with degree of %3.3f,%3.3f.\n",
					(*it)->id, p.m_X, p.m_Y, (*it)->next->id, p2.m_X, p2.m_Y, 
					(*it2)->id, q.m_X, q.m_Y, (*it2)->next->id, q2.m_X, q2.m_Y,
					param.first, param.second);
				return false;
			}
		}
	}
	return true;
}